/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 (1) 华磊。2017.6.13,　添加关节增量检查功能。
 ２）华磊。２017.6.21,安全检查不用脉冲单位，直接用rad ,m.
 ***************************************************************************
 *  @file InternalSafe.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "internalsafe.h"
#include <qdebug.h>
#include <math.h>

#define IF_DEBUG false

InternalSafe::InternalSafe(QVector<JointAttribute> *_robotAttribute, QVector<MotorData> *_motorDataVector, double ethercatDeltaTimeIn):
    motorDataVector(_motorDataVector)
{
    setComponentVersion( "InternalSafe", "2016-12-9", "1.0.0" );
    ethercatDeltaTime=ethercatDeltaTimeIn;
    jointsNum = (*_robotAttribute).size();
    positiveLimit.resize( jointsNum );
    negativeLimit.resize( jointsNum );
    speedLimit.resize( jointsNum );
    jogSpeedLimit.resize(jointsNum);
     accelerationLimit.resize(jointsNum);
    jointIncrementLimit.resize(jointsNum);
     jointIncrementIncrementLimit.resize(jointsNum);
     jogJointIncrementLimit.resize(jointsNum);
     jogJointIncrementIncrementLimit.resize(jointsNum);
    for(int i=0; i<jointsNum; i++)
    {
        if( 0==(*_robotAttribute)[i].jointType )
        {
            positiveLimit[i] =2*M_PI* (*_robotAttribute)[i].maxPosition  / 360.0 ;
            negativeLimit[i] =2*M_PI* (*_robotAttribute)[i].minPosition  / 360.0 ;
            speedLimit[i] =2*M_PI* (*_robotAttribute)[i].maxVelocity  / 360.0;
            accelerationLimit[i] =2*M_PI* (*_robotAttribute)[i].maxAccelation / 360.0 ;
            jointIncrementLimit[i]= speedLimit[i]*ethercatDeltaTime;

            //测试定义１r/s^2
             // jointIncrementIncrementLimit[i]= accelerationLimit[i]*ethercatDeltaTime*ethercatDeltaTime;
           jointIncrementIncrementLimit[i] = 10000*1 *ethercatDeltaTime*ethercatDeltaTime;
        }
        else
        {
            positiveLimit[i] = (*_robotAttribute)[i].maxPosition / 1000.0;
            negativeLimit[i] = (*_robotAttribute)[i].minPosition  / 1000.0;
            speedLimit[i] = (*_robotAttribute)[i].maxVelocity  / 1000.0;
            accelerationLimit[i] = (*_robotAttribute)[i].maxAccelation / 1000.0;
            jointIncrementLimit[i]= speedLimit[i]*ethercatDeltaTime;

             //测试定义１m/s^2 .screwPitch unit is mm
             //jointIncrementIncrementLimit[i]= accelerationLimit[i]*ethercatDeltaTime*ethercatDeltaTime;
            jointIncrementIncrementLimit[i] = 10000*1*ethercatDeltaTime*ethercatDeltaTime;

        }

        jogSpeedLimit[i]=0.11*speedLimit[i];
        jogJointIncrementLimit[i]=0.11*jointIncrementLimit[i];
        jogJointIncrementIncrementLimit[i]=10*jointIncrementIncrementLimit[i];//手动停止算法有问题，会导致加速度过大。
    }

    controlType=0;

}

int InternalSafe::setControlType(int controlTypeIn)
{
    controlType=controlTypeIn;
    return 1;
}


int InternalSafe::jointLimitCheck(const JntArray& jointsPosition, const JntArray& jointsPositionFeedback)
{
    int safePass = 0;
    stopJointList.clear();
    for(int i=0; i<jointsNum; i++)
     {
        if((jointsPosition(i)> positiveLimit[i]) && (jointsPosition(i)>jointsPositionFeedback(i)) )
        {
            if(IF_DEBUG)
            {
                qDebug()<< "InternalSafe::jointLimitCheck====Joint "<< i+1<<"exceed positive limit"<< jointsPosition(i)<< jointsPositionFeedback(i);
            }
            stopJointList.push_back(i);
            safePass = 1;
        }
        else if(  (jointsPosition(i)< negativeLimit[i]) && (jointsPosition(i)<jointsPositionFeedback(i)) )
        {
            if(IF_DEBUG)
            {
                qDebug()<< "InternalSafe::jointLimitCheck====Joint "<< i+1<<"exceed negative limit"<<  jointsPosition(i)<< jointsPositionFeedback(i);
            }
            stopJointList.push_back(i);
            safePass = -1;
        }
     }
     return safePass;
}


int InternalSafe::jointVelCheck(const JntArray& jointsVel)
{
    overspeedList.clear();
    int overspeed=0;
    for(int i=0; i<jointsNum; i++)
    {
        //非Ｔ１模式
        if( fabs(jointsVel(i)) > speedLimit[i]*1.05  && controlType>0)//超过最大速度
        {
            if(IF_DEBUG)
            {
                qDebug()<< "InternalSafe::jointVelCheck===Joint "<< i+1<<"超过最大速度"<<jointsVel(i)<<speedLimit[i];
            }
            overspeedList.push_back(i);
            overspeed = -1;
        }
         //Ｔ１模式
        if( fabs(jointsVel(i))  > jogSpeedLimit[i]  && controlType==0)//超过最大速度
        //if( fabs(jointsVel(i))  > speedLimit[i]  && controlType==0)//超过最大速度
        {
            if(IF_DEBUG)
            {
                qDebug()<< "InternalSafe::jointVelCheck===Joint "<< i+1<<"超过最大速度"<<jointsVel(i)<<speedLimit[i];
            }
            overspeedList.push_back(i);
            overspeed = -1;
        }
    }
    return overspeed;
}

int InternalSafe::jointIncrementCheck(std::vector<double> jointIncrement )
{
    if(jointsNum!=jointIncrement.size())
    {
        return 1;
    }
    jointIncrementAlarmList.clear();
    int overspeed=0;

    if(0==jointIncrement.size())
    {
        return 2;
    }

    for(int i=0; i<jointsNum; i++)
    {
        //非Ｔ１模式
        if( fabs( jointIncrement[i] )> jointIncrementLimit[i]*1.05  && controlType>0)//超过最大速度
        {
            if(IF_DEBUG)
            {
                qDebug()<< "InternalSafe::jointVelCheck===Joint "<< i+1<<"increment超过最大速度";
            }
            jointIncrementAlarmList.push_back(i);
            overspeed = -1;
        }
         //Ｔ１模式
        if( fabs(  jointIncrement[i] )> jogJointIncrementLimit[i]  && controlType==0)//超过最大速度
        {
            if(IF_DEBUG)
            {
                qDebug()<< "InternalSafe::jointVelCheck===Joint "<< i+1<<"increment超过最大速度";
            }
            jointIncrementAlarmList.push_back(i);
            overspeed = -1;
        }
    }
    return overspeed;
}


int InternalSafe::jointIncrementIncrementCheck(std::vector<double> jointIncrementIncrement)
{
    if(jointsNum!=jointIncrementIncrement.size())
    {
        return 1;
    }
    jointVelIncrementAlarmList.clear();
    int overspeed=0;

    if(0==jointIncrementIncrement.size())
    {
        return 2;
    }

    for(int i=0; i<jointsNum; i++)
    {
        //非Ｔ１模式
        if( fabs(  jointIncrementIncrement[i] ) > jointIncrementIncrementLimit[i]  && controlType>0)
        {
            if(IF_DEBUG)
            {
                qDebug()<< "InternalSafe::jointVelCheck===Joint "<< i+1<<"increment超过最大速度";
            }
            jointVelIncrementAlarmList.push_back(i);
            overspeed = -1;
        }
        //Ｔ１模式
        if( fabs(  jointIncrementIncrement[i] )> jogJointIncrementIncrementLimit[i]  && controlType>0)
        {
            if(IF_DEBUG)
            {
                qDebug()<< "InternalSafe::jointVelCheck===Joint "<< i+1<<"increment超过最大速度";
            }
            jointVelIncrementAlarmList.push_back(i);
            overspeed = -1;
        }
    }
    return overspeed;
}

